import matplotlib.pyplot as plt
from matplotlib.backends.backend_pdf import PdfPages
from math import sqrt
from matplotlib.lines import Line2D
#
def dataInputPDR(filename):
    result = []
    in_file = open(filename, 'r')
    lines = in_file.readlines()
    for line in lines:
        raw_data = line.split(' ')
        result.append([int(raw_data[0]), float(raw_data[1]), float(raw_data[2]), float(raw_data[3][:-1])])
    return result

def PDR_draw_trace(absolute_trajectory):
    return
    sx, sy, ex, ey = 0, 0, 0, 0
    figure, ax = plt.subplots()
    for i in range(len(absolute_trajectory) - 1):
        line1 = [(absolute_trajectory[i][0], absolute_trajectory[i][1]),
                 (absolute_trajectory[i + 1][0], absolute_trajectory[i + 1][1])]
        sx = min(sx, absolute_trajectory[i][0])
        ex = max(ex, absolute_trajectory[i][0])
        sy = min(sy, absolute_trajectory[i][1])
        ey = max(ey, absolute_trajectory[i][1])
        (line1_xs, line1_ys) = zip(*line1)
        ax.add_line(Line2D(line1_xs, line1_ys, linewidth=1, color='blue'))
    plt.xlim(sx, ex)
    plt.ylim(sy, ey)
    plt.plot()
    #plt.show()
    plt.savefig('tmp\\PDR.png')

def PDR_draw_trace_cv(absolute_trajectory, cv_ground_truth):
    return
    sx, sy, ex, ey = 0, 0, 0, 0
    figure, ax = plt.subplots()
    for i in range(len(absolute_trajectory) - 1):
        line1 = [(absolute_trajectory[i][0], absolute_trajectory[i][1]),
                 (absolute_trajectory[i + 1][0], absolute_trajectory[i + 1][1])]
        sx = min(sx, absolute_trajectory[i][0])
        ex = max(ex, absolute_trajectory[i][0])
        sy = min(sy, absolute_trajectory[i][1])
        ey = max(ey, absolute_trajectory[i][1])
        (line1_xs, line1_ys) = zip(*line1)
        ax.add_line(Line2D(line1_xs, line1_ys, linewidth=1, color='blue'))

    (kx, ky) = cv_ground_truth[0]
    for i in range(len(cv_ground_truth) - 13):
        xx1 = (cv_ground_truth[i][0] - kx) / 160
        yy1 = (cv_ground_truth[i][1] - ky) / 200
        y1 = -xx1
        x1 = yy1
        xx2 = (cv_ground_truth[i + 1][0] - kx) / 160
        yy2 = (cv_ground_truth[i + 1][1] - ky) / 200
        y2 = -xx2
        x2 = yy2
        line1 = [(x1, y1),
                 (x2, y2)]
        sx = min(sx, x1)
        ex = max(ex, x1)
        sy = min(sy, y1)
        ey = max(ey, y1)
        (line1_xs, line1_ys) = zip(*line1)
        ax.add_line(Line2D(line1_xs, line1_ys, linewidth=1, color='red'))
    plt.xlim(sx, ex)
    plt.ylim(sy, ey)
    plt.plot()
    #plt.show()
    plt.savefig('tmp\\PDR_cv.png')

def acc_draw(acc_data):
    tt = acc_data[0][0]
    st = []
    sa = []
    sb = []
    sc = []
    sd = []
    va = 0
    vb = 0
    vc = 0
    i = 0
    j = 0
    for (t, a, b, c) in acc_data:
        j = j + 1
        va = va + a
        vb = vb + b
        vc = vc + c
        while (t - acc_data[i][0] > 400):
            va = va - acc_data[i][1]
            vb = vb - acc_data[i][2]
            vc = vc - acc_data[i][3]
            i = i + 1
        if t-tt < 10000 or t-tt > 20000:
            continue
        st.append(t - tt)
        sa.append(va / (j - i))
        sb.append(vb / (j - i))
        sc.append(vc / (j - i))
        sd.append(sqrt((va / (j - i)) ** 2 + (vb / (j - i)) ** 2 + (vc / (j - i) - 9.8) ** 2))

    pdf = PdfPages('PF_result/acc.pdf')
    plt.clf()
    fig, ax = plt.subplots(nrows=3, ncols=1, sharex='all', sharey='none')
    fig.text(0.5, 0.04, 'Time(ms)', ha='center')
    #fig.text(0.04, 0.5, 'Gyroscope', va='center', rotation='vertical')
    fig.text(0.04, 0.5, 'Acceleration', va='center', rotation='vertical')
    ax[0].plot(st, sa)
    ax[1].plot(st, sb)
    ax[2].plot(st, sc)
    #ax[3].plot(st, sd)
    pdf.savefig()
    plt.close()
    pdf.close()

